#ifdef __cplusplus
extern "C"{
#endif
#include "xs.h"

#define __MALLOC_TEST_ONLY_


static t_queue_global q_data;
static unsigned long mem_alloc_count = 0;


#ifdef __MALLOC_TEST_ONLY_
void xs_show_malloc_count()
{
    xs_logd("memory alloc count :%ld \n",mem_alloc_count);
}
#endif

void *xs_mem_malloc(unsigned long size) 
{
	void *memp = xs_malloc(size);

	if(memp == NULL)
		xs_logd("sys_malloc() , malloc()failed ! S %ld \n",size);

	mem_alloc_count++;

	return (memp);
}
void xs_mem_free(void *memp) 
{
    if(memp == NULL)
        xs_logd("mem_free (): NULL memp");
    else
        xs_free(memp);
    
    mem_alloc_count--;
}

MQUE_RET xs_msgque_mutex_create(pthread_mutex_t *pmutex) 
{
    if(pthread_mutex_init(pmutex,NULL) != 0)
    {
        xs_logd("msgque_mutex_create() ; pthread_mutex_init() failed \n");
        return (MQUE_RET_FAIL);
    }
	
    return (MQUE_RET_OK);
}
int xs_msgque_mutex_lock(pthread_mutex_t *pmutex)
{
    if(pthread_mutex_lock(pmutex) != 0)
    {
        xs_logd("msgque_mutex_lock(); pthread_mutex_lock() failed \n");
        return (MQUE_RET_FAIL);
    }

    return (MQUE_RET_OK);
}


int xs_msgque_mutex_unlock(pthread_mutex_t *pmutex)
{
    if(pthread_mutex_unlock(pmutex) != 0)
    {
        xs_logd("msgque_mutex_unlock(); pthread_mutex_unlock() failed !\n");
        return (MQUE_RET_FAIL);
    }

    return (MQUE_RET_OK);
}

int xs_msgque_init(void)
{
    if(pthread_mutex_init(&q_data.g_mutex,NULL) != 0)
    {
        xs_logd("sys_msgque_init(), msgque_mutex_create(q_data.g_mutex) failed !\n");
        return (MQUE_RET_FAIL);
    }

    if(pthread_mutex_init(&q_data.g_que_mutex,NULL) != 0)
    {
        xs_logd("sys_msgque_init(), msgque_mutex_create(que_mutex) failed !\n");
        return (MQUE_RET_FAIL);
    }
	
    if(pthread_mutex_init(&q_data.g_sem_mutex,NULL))
    {
        xs_logd("sys_msgque_init(), msgque_mutex_create(sem_mutex) failed !\n");
        return (MQUE_RET_FAIL);
    }

    q_data.que_num = 0;
    q_data.semnum = 0;

    if((netrecv_id = xs_msgque_create("net_recv")) == MQUE_RET_FAIL)
    {
        xs_logd("sys_msgque_init(); msg_net_recv init failed!\n");
        return MQUE_RET_FAIL;
    }   

    if((netsend_id = xs_msgque_create("net_send")) == MQUE_RET_FAIL)
    {
        xs_logd("sys_msgque_init(); msg_net_send init failed!\n");
        return MQUE_RET_FAIL;
    }   

    if((ppcrecv_id = xs_msgque_create("uart_recv")) == MQUE_RET_FAIL)
    {
        xs_logd("sys_msgque_init(); msg_ppc_recv init failed!\n");
        return MQUE_RET_FAIL;
    }   

    if((ppcsend_id = xs_msgque_create("uart_recv")) == MQUE_RET_FAIL)
    {
        xs_logd("sys_msgque_init(); msg_ppc_send init failed!\n");
        return MQUE_RET_FAIL;
    }   
	
    return MQUE_RET_OK;
}

int xs_msgque_sem_create(char *pname,t_semid *psemid)
{
	t_scb *pscb = NULL;
	int semindex;
	*psemid = (t_semid)NULL;

	if(pname == NULL)
	{
		xs_logd("msgque_sem_create() pname == NULL !\n");
		return (MQUE_RET_ARG_ERROR);
	}
	xs_msgque_mutex_lock(&q_data.g_sem_mutex);
	semindex = q_data.semnum;  
	if(semindex >= OS_WRAP_MAX_SEMS)
	{
		xs_msgque_mutex_unlock(&q_data.g_sem_mutex);
		xs_logd("msgque_sem_create(), semindex > max sems limit \n");

		return (MQUE_RET_FAIL);
	}
	pscb = &(q_data.scb_array[semindex]);  
	if(xs_msgque_mutex_create(&pscb->sem_mutex) != MQUE_RET_OK)
	{
		xs_msgque_mutex_unlock(&q_data.g_sem_mutex);
		xs_logd("msgque_sem_create(),msgque_mutex_create(&pscb->sem_mutex) failed\n");

		return (MQUE_RET_FAIL);
	}
	if(pthread_cond_init(&pscb->cond_var,NULL) != 0)    
	{
		pthread_mutex_destroy(&pscb->sem_mutex);
		xs_msgque_mutex_unlock(&q_data.g_sem_mutex);
		xs_logd("msgque_sem_create(),pthread_cond_init(&pscb->cond_var,NULL) failed\n");

		return (MQUE_RET_FAIL);
	}

	pscb->semid = semindex + 1; 
	pscb->sem_count = 0;
	q_data.semnum++;
	*psemid = pscb->semid;
	xs_memcpy(pscb->name,pname,sizeof(pscb->name) - 1);
	xs_msgque_mutex_unlock(&q_data.g_sem_mutex);
	
	return (MQUE_RET_OK);
}

int xs_msgque_sem_release(t_semid semid)
{
	t_scb *pscb;
	int semindex;

	for(semindex=0;semindex < q_data.semnum;semindex++)
		if(q_data.scb_array[semindex].semid == semid)
			break;

	if(semindex >= q_data.semnum)
	{
		xs_logd("msgque_sem_release(); semindex > q_data.semnum ,No such semid ! \n");
		return (MQUE_RET_NO_EXIST);
	}
	
	pscb = &q_data.scb_array[semindex];
	xs_msgque_mutex_lock(&pscb->sem_mutex);
	pscb->sem_count++;  
	
	if(pscb->sem_count == 1)
		pthread_cond_signal(&pscb->cond_var);  
	
	xs_msgque_mutex_unlock(&pscb->sem_mutex);
	
	return (MQUE_RET_OK);
}

int xs_msgque_sem_wait(t_semid semid)
{
	t_scb *pscb;
	int semindex;

	for(semindex = 0;semindex < q_data.semnum;semindex++)
		if(q_data.scb_array[semindex].semid == semid)
			break;
	if(semindex >= q_data.semnum)
	{
		xs_logd("msgque_sem_wait(); semindex > q_data.semnum ,No such semid ! \n");
		return (MQUE_RET_NO_EXIST);
	}

	pscb = &q_data.scb_array[semindex];

	xs_msgque_mutex_lock(&pscb->sem_mutex);

	if(pscb->sem_count == 0)  
		if(pthread_cond_wait(&pscb->cond_var,&pscb->sem_mutex) != 0)
			return MQUE_RET_FAIL;
		
	if(pscb->sem_count > 0)
		pscb->sem_count--;  
	
	xs_msgque_mutex_unlock(&pscb->sem_mutex);

	return (MQUE_RET_OK);
}

unsigned long xs_msgque_create(char *p_name)
{
    t_qcb *pqcb = NULL;
    int qindex;
    t_queid queid;

    if(p_name == NULL)
    {
        xs_logd("sys_msgque_create();p_name == NULL \n");
        return (MQUE_RET_FAIL);
    }
    xs_msgque_mutex_lock(&q_data.g_que_mutex);
    qindex = q_data.que_num; 
    if(qindex >= OS_WRAP_MAX_QUEUES)
    {
        xs_msgque_mutex_unlock(&q_data.g_que_mutex);
        xs_logd("sys_msgque_create(); qindex >= OS_WARP_MAX_QUEUES \n");

        return (MQUE_RET_FAIL);
    }

    pqcb = &(q_data.qcb_array[qindex]);

    if(xs_msgque_sem_create(p_name,&pqcb->semid) != MQUE_RET_OK)
    {
        xs_logd("sys_msgque_create(); msgque_sem_create() != 0 \n");
        xs_msgque_mutex_unlock(&q_data.g_que_mutex);
        return (MQUE_RET_FAIL);
    }

    if(xs_msgque_mutex_create(&pqcb->qmutex) != MQUE_RET_OK)
    {
        xs_logd("sys_msgque_create(); msgque_mutex_create() != 0 \n");
        xs_msgque_mutex_unlock(&q_data.g_que_mutex);
        return (MQUE_RET_FAIL);
    }
    pqcb->firstp = pqcb->lastp;    
    q_data.que_num++;
    queid = qindex + 1;    
    xs_memcpy(pqcb->name,p_name,sizeof(pqcb->name)-1);   
    pqcb->name[sizeof(pqcb->name)-1] = '\x0';
    xs_msgque_mutex_unlock(&q_data.g_que_mutex);

    return queid;

}

unsigned short xs_msgque_send(unsigned long qid,void *msg,unsigned short msglen)
{
    t_msg_block *pbmsg;
    t_qcb *pqcb;
    unsigned long qindex;

    if(msg == NULL)
    {
        xs_logd("sys_msgque_send(),msg == NULL \n");
        return (MQUE_RET_ARG_ERROR);
    }

    qindex = qid - 1;

    if(qindex >= OS_WRAP_MAX_QUEUES)
    {
        xs_logd("sys_msgque_send();qid error\n");
        return (MQUE_RET_ARG_ERROR);
    }

    pqcb = &(q_data.qcb_array[qindex]);

    pbmsg = (t_msg_block *)xs_mem_malloc(sizeof(t_msg_block));   

    if(pbmsg == NULL)
    {
        xs_logd("sys_msgque_send(),mem_malloc pbmsg == NULL \n");
        return (MQUE_RET_FAIL);
    }

    pbmsg->nextp = NULL;    
    pbmsg->msg = msg;   
    pbmsg->msglen = msglen;  
	//xs_logd("%s\n", pbmsg ->msg);

	xs_msgque_mutex_lock(&pqcb->qmutex);

    if(pqcb->lastp != NULL)
    {
        pqcb->lastp->nextp = pbmsg;
        pqcb->lastp = pbmsg;
    }
    else
    {
        pqcb->firstp = pqcb->lastp = pbmsg;
    }

    xs_msgque_sem_release(pqcb->semid); 

    xs_msgque_mutex_unlock(&pqcb->qmutex);

    return (MQUE_RET_SUCCESS);

}
unsigned short xs_msgque_recv(unsigned long qid,void **msg,unsigned short *msglen)
{
    t_msg_block *pbmsg;
    t_qcb *pqcb;
    unsigned long qindex;

    pbmsg = NULL;
    qindex = qid - 1;

    if(qindex >= OS_WRAP_MAX_QUEUES)
    {
        xs_logd("sys_msgque_recv(); qindex >= limit \n");
        return (MQUE_RET_ARG_ERROR);
    }

    pqcb = &q_data.qcb_array[qindex];

    if(xs_msgque_sem_wait(pqcb->semid) != MQUE_RET_OK)
    {
        xs_logd("sys_msgque_recv(); msgque_sem_wait () failed \n");
        return (MQUE_RET_FAIL);
    }

    xs_msgque_mutex_lock(&pqcb->qmutex);

    pbmsg = pqcb->firstp;   

    if(pbmsg == NULL)
    {
        xs_logd("sys_msgque_recv(); pbmsg == NULL \n");
        xs_msgque_mutex_unlock(&pqcb->qmutex);
        return (MQUE_RET_FAIL);
    }

    *msg = pbmsg->msg;   
    *msglen = pbmsg->msglen;   

    if(pqcb->lastp == pbmsg) 
    {
        pqcb->lastp = NULL;
    }
    pqcb->firstp = pbmsg->nextp;

    xs_mem_free(pbmsg); 
    xs_msgque_mutex_unlock(&pqcb->qmutex);

    return (MQUE_RET_SUCCESS);

}



#ifdef __cplusplus
}
#endif
